
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_view.c
  * @author     baiyang
  * @date       2021-9-3
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "ahrs_view.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void vehicle_home_cb(void *parameter);
static void update_location_and_state();
/*----------------------------------variable----------------------------------*/
static ahrs_view ahrs;

static uitc_vehicle_origin origin_pos_g;
static uitc_vehicle_attitude vehicle_attitude;
static uitc_sensor_gyr sensor_gyr;
static uitc_sensor_acc sensor_acc;
static uitc_sensor_mag sensor_mag;
static uitc_vehicle_alt alt_info;
static uitc_sensor_baro sensor_baro;
static uitc_vehicle_position pos_info;

static itc_node_t vehicle_home_node;
static itc_node_t origin_pos_node;

static Location curr_loc_tmp;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
ahrs_view* get_ahrs_view()
{
    return &ahrs;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_view_init()
{
    vehicle_home_node = itc_subscribe(ITC_ID(vehicle_home), vehicle_home_cb);
}

/**
  * @brief       
  * @param[in]   ahrs  
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_view_update()
{
    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);
    itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr);
    itc_copy_from_hub(ITC_ID(sensor_acc), &sensor_acc);
    itc_copy_from_hub(ITC_ID(sensor_mag), &sensor_mag);
    itc_copy_from_hub(ITC_ID(vehicle_alt), &alt_info);
    itc_copy_from_hub(ITC_ID(sensor_baro), &sensor_baro);
    itc_copy_from_hub(ITC_ID(vehicle_position), &pos_info);

    ahrs.roll_sensor_cd = Rad2Deg(vehicle_attitude.vehicle_euler.roll * 100);
    ahrs.pitch_sensor_cd = Rad2Deg(vehicle_attitude.vehicle_euler.pitch * 100);
    ahrs.yaw_sensor_cd = Rad2Deg(vehicle_attitude.vehicle_euler.yaw * 100);

    ahrs.cos_roll = cosf(vehicle_attitude.vehicle_euler.roll);
    ahrs.cos_pitch = cosf(vehicle_attitude.vehicle_euler.pitch);
    ahrs.cos_yaw = cosf(vehicle_attitude.vehicle_euler.yaw);

    ahrs.sin_roll = sinf(vehicle_attitude.vehicle_euler.roll);
    ahrs.sin_pitch = sinf(vehicle_attitude.vehicle_euler.pitch);
    ahrs.sin_yaw = sinf(vehicle_attitude.vehicle_euler.yaw);

    ahrs.attitude_body = vehicle_attitude.vehicle_quat;

    ahrs.roll = vehicle_attitude.vehicle_euler.roll;
    ahrs.pitch = vehicle_attitude.vehicle_euler.pitch;
    ahrs.yaw = vehicle_attitude.vehicle_euler.yaw;
    
    ahrs.gyr.x = sensor_gyr.sensor_gyr_correct[0];
    ahrs.gyr.y = sensor_gyr.sensor_gyr_correct[1];
    ahrs.gyr.z = sensor_gyr.sensor_gyr_correct[2];

    ahrs.acc.x = sensor_acc.sensor_acc_correct[0];
    ahrs.acc.y = sensor_acc.sensor_acc_correct[1];
    ahrs.acc.z = sensor_acc.sensor_acc_correct[2];

    update_location_and_state();

    ahrs.curr_alt_amsl   = alt_info.alt;
    ahrs.home_alt_above_origin = ahrs.home_valid_alt ? ahrs.home_alt_amsl- ahrs.origin_alt_amsl : 0.0f;

    ahrs.accel_ef.x = pos_info.ax;
    ahrs.accel_ef.y = pos_info.ay;
    ahrs.accel_ef.z = -alt_info.az - GRAVITY_MSS;

    ahrs.relpos_cm.x = pos_info.x*100.0f;
    ahrs.relpos_cm.y = pos_info.y*100.0f;
    ahrs.relpos_cm.z = alt_info.relative_alt * 100.0f;

    ahrs.velocity_cm.x = pos_info.vx*100.0f;
    ahrs.velocity_cm.y = pos_info.vy*100.0f;
    ahrs.velocity_cm.z = alt_info.vz * 100.0f;
}

static void vehicle_home_cb(void *parameter)
{
    uitc_vehicle_home vehicle_home = *((uitc_vehicle_home *)parameter);

    ahrs.home_is_set = vehicle_home.valid_lpos;
    ahrs.home_valid_alt = vehicle_home.valid_alt;
    ahrs.home_alt_amsl   = vehicle_home.alt;

    location_from_lat_lon_alt(&ahrs.home_loc, vehicle_home.lat, vehicle_home.lon, vehicle_home.alt*100.0f, ALT_FRAME_ABSOLUTE);
}

static void update_location_and_state()
{
    if (itc_copy_from_hub(ITC_ID(vehicle_origin), &origin_pos_g) != 0) {
        return;
    }

    if (ahrs.origin_loc.lat != origin_pos_g.lat
        && ahrs.origin_loc.lng != origin_pos_g.lon
        && origin_pos_g.valid_lpos) {
        location_from_lat_lon_alt(&ahrs.origin_loc, origin_pos_g.lat, origin_pos_g.lon, origin_pos_g.alt*100.0f, ALT_FRAME_ABSOLUTE);
        location_from_lat_lon_alt(&ahrs.curr_loc, origin_pos_g.lat, origin_pos_g.lon, origin_pos_g.alt*100.0f, ALT_FRAME_ABSOLUTE);
    }

    if (origin_pos_g.valid_lpos) {
        curr_loc_tmp = ahrs.origin_loc;

        location_offset(&curr_loc_tmp, pos_info.x, pos_info.y);
        location_set_alt_cm(&curr_loc_tmp, alt_info.relative_alt*100.0f, ALT_FRAME_ABOVE_HOME);

        ahrs.curr_loc = curr_loc_tmp;
    }

    if (ahrs.origin_lat != origin_pos_g.lat) {
        ahrs.origin_lat = origin_pos_g.lat;
    }

    if (ahrs.origin_lng != origin_pos_g.lon) {
        ahrs.origin_lng = origin_pos_g.lon;
    }

    if (!math_flt_equal(ahrs.origin_alt_amsl, origin_pos_g.alt)) {
        ahrs.origin_alt_amsl = origin_pos_g.alt;
    }

    ahrs.valid_alt = origin_pos_g.valid_alt;
    ahrs.valid_hpos = origin_pos_g.valid_hpos;
    ahrs.valid_lpos = origin_pos_g.valid_lpos;
}

/*------------------------------------test------------------------------------*/


